#include <cmath>
#include "astar.h"
#include "iostream"

void Astar::InitAstar(std::vector<std::vector<int>> &_maze)
{
    maze = _maze;
}

int Astar::calcG(Point *temp_start, Point *point)
{
    int extraG = (abs(point->x - temp_start->x) + abs(point->y - temp_start->y)) == 1 ? kCost1 : kCost2;
    int parentG = point->parent == nullptr ? 0 : point->parent->G; //�����ǳ�ʼ�ڵ㣬���丸�ڵ��ǿ�
    return parentG + extraG;
}

int Astar::calcH(Point *point, Point *end)
{
    //�ü򵥵�ŷ�����þ�������H������H�ļ����ǹؼ������кܶ��㷨��û�����о�^_^
    return sqrt((double)(end->x - point->x)*(double)(end->x - point->x) + (double)(end->y - point->y)*(double)(end->y - point->y))*kCost1;
}

int Astar::calcF(Point *point)
{
    return point->G + point->H;
}

Point *Astar::getLeastFpoint()
{
    if (!openList.empty())
    {
        auto resPoint = openList.front();
        for (auto &point : openList)
            if (point->F<resPoint->F)
                resPoint = point;
        return resPoint;
    }
    return nullptr;
}

/*
* function:clear the list
* author:wuxy
* data:20180611
*/
void Astar::clearList()
{
    if(!openList.empty())
    {
        for(auto &p:openList)
            if(p!=nullptr)
                p=nullptr;
        openList.clear();
    }

    if(!closeList.empty())
    {
        for(auto &p:closeList)
            if(p!=nullptr)
                p=nullptr;
        closeList.clear();
    }
}

Point *Astar::findPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner)
{
    clearList();  //clear list before operate list
    std::cout<<__FUNCTION__<<" line: "<<__LINE__<<std::endl;
    openList.push_back(new Point(startPoint.x, startPoint.y)); //��������,��������һ���ڵ㣬��������
    while (!openList.empty())
    {
        auto curPoint = getLeastFpoint(); //�ҵ�Fֵ��С�ĵ�
        openList.remove(curPoint); //�ӿ����б���ɾ��
        closeList.push_back(curPoint); //�ŵ��ر��б�
        //1,�ҵ���ǰ��Χ�˸����п���ͨ���ĸ���
        auto surroundPoints = getSurroundPoints(curPoint, isIgnoreCorner);
        for (auto &target : surroundPoints)
        {
            //2,��ĳһ�����ӣ����������ڿ����б��У����뵽�����б������õ�ǰ��Ϊ�丸�ڵ㣬����F G H
            if (!isInList(openList, target))
            {
                target->parent = curPoint;

                target->G = calcG(curPoint, target);
                target->H = calcH(target, &endPoint);
                target->F = calcF(target);

                openList.push_back(target);
            }
            //3����ĳһ�����ӣ����ڿ����б��У�����Gֵ, ������ԭ���Ĵ�, ��ʲô������, �����������ĸ��ڵ�Ϊ��ǰ��,������G��F
            else
            {
                int tempG = calcG(curPoint, target);
                if (tempG<target->G)
                {
                    target->parent = curPoint;

                    target->G = tempG;
                    target->F = calcF(target);
                }
            }
            Point *resPoint = isInList(openList, &endPoint);
            if (resPoint)
                return resPoint; //�����б����Ľڵ�ָ�룬��Ҫ��ԭ��������endpointָ�룬��Ϊ���������
        }
    }

    return nullptr;
}

std::list<Point *> Astar::GetPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner)
{
    Point *result = findPath(startPoint, endPoint, isIgnoreCorner);
    std::list<Point *> path;
    //����·��������û�ҵ�·�������ؿ�����
    while (result)
    {
        path.push_front(result);
        result = result->parent;
    }

    return path;
}

Point *Astar::isInList(const std::list<Point *> &list, const Point *point) const
{
    //�ж�ĳ���ڵ��Ƿ����б��У����ﲻ�ܱȽ�ָ�룬��Ϊÿ�μ����б����¿��ٵĽڵ㣬ֻ�ܱȽ�����
    for (auto p : list)
        if (p->x == point->x&&p->y == point->y)
            return p;
    return nullptr;
}

bool Astar::isCanreach(const Point *point, const Point *target, bool isIgnoreCorner) const
{
    if (target->x<0 || target->x>maze.size() - 1
        || target->y<0 || target->y>maze[0].size() - 1  //������ͼ
        || maze[target->x][target->y] == 1          //�ϰ���
        || (target->x == point->x&&target->y == point->y)
        || isInList(closeList, target)) //�������뵱ǰ�ڵ��غϡ�������ͼ�����ϰ�������ڹر��б��У�����false
        return false;
    else
    {
        if (abs(point->x - target->x) + abs(point->y - target->y) == 1) //��б�ǿ���
            return true;
        else
        {
            //б�Խ�Ҫ�ж��Ƿ���ס
            if (maze[point->x][target->y] == 0 && maze[target->x][point->y] == 0)
                return true;
            else
                return isIgnoreCorner;
        }
    }
}

std::vector<Point *> Astar::getSurroundPoints(const Point *point, bool isIgnoreCorner) const
{
    std::vector<Point *> surroundPoints;

    for (int x = point->x - 1; x <= point->x + 1; x++)
        for (int y = point->y - 1; y <= point->y + 1; y++)
            if (isCanreach(point, new Point(x, y), isIgnoreCorner))
                surroundPoints.push_back(new Point(x, y));

    return surroundPoints;
}
